#ifndef ALGORITHM_MATH_H
#define ALGORITHM_MATH_H

#include <random>
#include <vector>
#include "Eigen/Dense"

using namespace Eigen;

#define TEST_PREC 1e-8
#define EPS_DOUBLE EPS_16
#define EPS_FLOAT EPS_8
#define EPS_16 1e-16
#define EPS_15 1e-15
#define EPS_8 1e-8
#define EPS_7 1e-7
#define EPS_6 1e-6
#define EPS_5 1e-5
#define EPS_4 1e-4
#define EPS_3 1e-3
#define EPS_2 1e-2
#define EPS_1 1e-1


namespace ZORAL
{
    typedef Matrix<double, 6, 1> Vector6d;
    typedef Matrix<double, 7, 1> Vector7d;
    typedef Matrix<double, 6, 6> Matrix6d;

    Matrix3d Skew(const Vector3d &v);
    Matrix3d SkewSkew(const Vector3d &v);

    template<typename T>
    int Sign(const T &x)
    {
        return (x > 0) ? 1 : (x < 0) ? -1 : 0;
    }

    template<typename T>
    T ZoralAtan2(const T &x, const T &y)
    {
        return (std::abs(x) < EPS_FLOAT && std::abs(y) < EPS_FLOAT) ? 0.0 : atan2(x, y);
    }

    double Random(const double &lower, const double &upper);
    VectorXd FindClosestVector(const std::vector<VectorXd> &vector_sets, const VectorXd &vector);
    std::vector<std::vector<double>> SlerpQuaternions(const std::vector<double> &q1, const std::vector<double> &q2,
                                                      const int &steps);

    template<typename T>
    T Rad2deg(const T &radians)
    {
        return radians * (180.0 / M_PI);
    }

    template<typename T>
    T Deg2rad(const T &degrees)
    {
        return degrees * (M_PI / 180);
    }

    template<typename T>
    T Squared(const T &d) {
        return d * d;
    }

    double QuinticTimeScaling(const double &Tf, const double &t);

    //! 计算四元数的速度
    Eigen::Vector3d QuatVel(const Eigen::Quaterniond &q1, const Eigen::Quaterniond &q2, const double &dt);

    Eigen::MatrixXd PseudoInverse(const Eigen::MatrixXd &mat, const double &tolerance = 1e-6);

} // namespace ZORAL

#endif // ALGORITHM_MATH_H
